//ros库
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>//用来进行发送目标位置,带有头消息和位姿的消息
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/TwistWithCovariance.h>
#include <mavros_msgs/CommandBool.h>//用来进行无人机解锁,发送True则为解锁，返回success值为True表示上电/断电操作执行成功，返回值result(int32)，如果解锁/上锁操作成功执行，result值为0

//SetMode服务的头文件，该服务的类型为mavros_msgs::SetMode
//用来设置无人机的飞行模式，切换offboard
/*
wiki上的消息定义如下
# set FCU mode
#
# Known custom modes listed here:
# http://wiki.ros.org/mavros/CustomModes

# basic modes from MAV_MODE
uint8 MAV_MODE_PREFLIGHT = 0
uint8 MAV_MODE_STABILIZE_DISARMED = 80
uint8 MAV_MODE_STABILIZE_ARMED = 208
uint8 MAV_MODE_MANUAL_DISARMED = 64
uint8 MAV_MODE_MANUAL_ARMED = 192
uint8 MAV_MODE_GUIDED_DISARMED = 88
uint8 MAV_MODE_GUIDED_ARMED = 216
uint8 MAV_MODE_AUTO_DISARMED = 92
uint8 MAV_MODE_AUTO_ARMED = 220
uint8 MAV_MODE_TEST_DISARMED = 66
uint8 MAV_MODE_TEST_ARMED = 194

uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != ''
string custom_mode # string mode representation or integer
---
bool success

String类型的变量custom_mode就是我们想切换的模式，有如下选择：
MANUAL，ACRO，ALTCTL，POSCTL，OFFBOARD，STABILIZED，RATTITUDE，AUTO.MISSION
AUTO.LOITER，AUTO.RTL，AUTO.LAND，AUTO.RTGS，AUTO.READY，AUTO.TAKEOFF
*/
#include <mavros_msgs/SetMode.h>
//订阅的消息体的头文件，该消息体的类型为mavros_msgs::State
#include <mavros_msgs/State.h> 

#include <mavros_msgs/PositionTarget.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>//可用来存储Double类型的数据
#include <sstream>
#include <tf2/LinearMath/Quaternion.h>


mavros_msgs::State current_state;//建立一个订阅消息体类型的变量，用于存储订阅的信息
geometry_msgs::PoseStamped local_pos;//用于存储订阅到的无人机的姿态信息（位置等）
geometry_msgs::PoseStamped last_local_pos;//上次订阅到的无人机的姿态信息（位置等）
//ConstPtr：常量智能指针，指向该消息的常量实例。这意味着你不能修改通过这个指针访问的对象。使用 ConstPtr 有助于确保消息在回调中保持不变，避免意外修改。
void state_cb(const mavros_msgs::State::ConstPtr& msg)//订阅时的回调函数，接受到该消息体的内容时执行里面的内容，内容是储存飞控当前的状态
{
    current_state = *msg;
}

void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    last_local_pos = local_pos; // 保存上次的姿态信息
    local_pos = *msg;           // 更新当前的姿态信息
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node"); //ros系统的初始化，argc和argv在后期节点传值会使用，最后一个参数offb_node为节点名称
    ros::NodeHandle nh;//实例化ROS句柄，这个ros::NodeHandle类封装了ROS中的一些常用功能
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);//这是一个订阅者对象，可以订阅无人机的状态信息（状态信息来源为MAVROS发布），用来储存无人机的状态，在回调函数中会不断更新这个状态变量
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, local_pos_callback);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);//这是一个发布者对象，用来在本地坐标系下发布目标点，后面会以20Hz频率发布目标点
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");//一个客户端，用来解锁无人机，这是因为无人机如果降落后一段时间没有收到信号输入，会自动上锁来保障安全
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");//一个客户端，用来切换飞行模式
    
    ros::Rate rate(20.0);

    while(ros::ok() && !current_state.connected)// 等待飞控和MAVROS建立连接，current_state是我订阅的MAVROS的状态，在收到心跳包之后连接成功跳出循环
    {
        ros::spinOnce();
        rate.sleep();
    }

    //定义一个变量来存储要设置的飞行模式
    /*
        可以设置的飞行模式为：
        MANUAL  手动
        ACRO    特技
        ALTCTL  高度
        POSCTL  定点
        OFFBOARD    外部模式
        STABILIZED  自稳
        RATTITUDE
        AUTO.LOITER     保持当前位置模式
        AUTO.RTL        自动返航模式
        AUTO.TAKEOFF    自动起飞模式
        AUTO.LAND       自动降落模式
        AUTO.MISSION    执行预设航点任务模式
        AUTO.RTGS       自动返回到航点（Return to GPS）
        AUTO.READY      准备模式。此模式表示无人机已准备好执行任务，但尚未启动。
    */
    mavros_msgs::State my_px4_mode;
    my_px4_mode.mode = "OFFBOARD";        //默认模式为“ POSCTL ”（定点模式）
    //建立一个类型为SetMode的服务端offb_set_mode，并将其中的模式mode设为"my_px4_mode.mode"，作用便是用于后面的客户端与服务端之间的通信（服务）
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = my_px4_mode.mode;
    ROS_INFO("want my_px4_mode is %s", my_px4_mode.mode.c_str());

    //建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的客户端与服务端之间的通信（服务）
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request_time = ros::Time::now();//更新判断模式时间
    ros::Time last_local_pos_time = ros::Time::now();//更新记录无人机姿态（如位置）的时间
    std_msgs::Float64 pos_errorx[3];//记录x轴误差,pos_error[0]表示这次误差，以此类推
    std_msgs::Float64 pos_errory[3];//记录y轴误差,pos_error[0]表示这次误差，以此类推
    std_msgs::Float64 pos_errorz[3];//记录z轴误差,pos_error[0]表示这次误差，以此类推
    for (int i = 0; i < 3; ++i) //初始化误差为10000米
    {
        pos_errorx[i].data = 10000;
        pos_errory[i].data = 10000;
        pos_errorz[i].data = 10000;
    }

    geometry_msgs::PoseStamped pose;//实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
    pose.pose.position.x = 0;//尽管PX4在航空航天常用的NED坐标系下操控飞机，但MAVROS将自动将该坐标系切换至常规的ENU坐标系下
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    tf2::Quaternion q;
    q.setRPY(0, 0, -3.1415926);//float型数据，第三个数是偏航角（弧度），无人机从上往下看逆时针变大
    pose.pose.orientation.x = q.x();
    pose.pose.orientation.y = q.y();
    pose.pose.orientation.z = q.z();
    pose.pose.orientation.w = q.w();

    //在进入Offboard模式之前，必须已经启动了local_pos_pub数据流，否则模式切换将被拒绝。
    //这里的100可以被设置为任意数
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    pose.pose.position.x = local_pos.pose.position.x;
    pose.pose.position.y = local_pos.pose.position.y;
    pose.pose.position.z = local_pos.pose.position.z - 10;     //无人机高度提高2米
    
    if( current_state.mode != my_px4_mode.mode)//首先判断当前模式是否为offboard模式，如果不是，则进入if语句内部
    {
        offb_set_mode.request.custom_mode = "OFFBOARD";
        if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)//客户端set_mode_client向服务端offb_set_mode发起请求call，然后服务端回应response将模式返回，这就打开了my_px4_mode.data模式
        {
            ROS_INFO("%s enabled", my_px4_mode.mode.c_str());//打开Offboard模式后在终端打印信息
        }
        last_request_time = ros::Time::now();//更新时间
    }
    if( !current_state.armed)//判断当前状态是否解锁，如果没有解锁，则进入if语句内部
    {
        //客户端arming_client向服务端arm_cmd发起请求call，然后服务端回应response成功解锁，则解锁成功
        if( arming_client.call(arm_cmd) && arm_cmd.response.success)
        {
            //解锁后在终端打印信息
            ROS_INFO("Vehicle armed");
            //更新时间
            last_request_time = ros::Time::now();
        }
        last_request_time = ros::Time::now();//更新时间
    }

    //大循环，只要节点还在ros::ok()的值就为正
    while(ros::ok())
    {

        //发布位置信息，所以综上飞机只有先打开offboard模式然后解锁才能飞起来
        
        local_pos_pub.publish(pose); 
        if(ros::Time::now() - last_local_pos_time > ros::Duration(0.8))//每隔一段时间记录一下与目标位置的差值，若连续3次在误差内，则认为已经到达目标位置
        {
            pos_errorx[2].data = pos_errorx[1].data;//刷新误差
            pos_errory[2].data = pos_errory[1].data;
            pos_errorz[2].data = pos_errorz[1].data;

            pos_errorx[1].data = pos_errorx[0].data;
            pos_errory[1].data = pos_errory[0].data;
            pos_errorz[1].data = pos_errorz[0].data;

            pos_errorx[0].data = local_pos.pose.position.x - pose.pose.position.x;
            pos_errory[0].data = local_pos.pose.position.y - pose.pose.position.y;
            pos_errorz[0].data = local_pos.pose.position.z - pose.pose.position.z;
            
            ROS_WARN("local_pos.x、y、z：%f,%f,%f",local_pos.pose.position.x,local_pos.pose.position.y,local_pos.pose.position.z);
            ROS_INFO("pos_errorx[0].data:%f,pos_errory[0].data:%f,pos_errorz[0].data:%f",pos_errorx[0].data,pos_errory[0].data,pos_errorz[0].data);

            bool all_in_range = true;
            for (int i = 0; i < 3; ++i) {
                if (!(pos_errorx[i].data > -0.6 && pos_errorx[i].data < 0.6) ||
                    !(pos_errory[i].data > -0.6 && pos_errory[i].data < 0.6) ||
                    !(pos_errorz[i].data > -0.6 && pos_errorz[i].data < 0.6)) {
                    all_in_range = false;
                    break; // 只要有一个不满足条件，就可以退出循环
                }
            }
            if (all_in_range) {
                // 所有数据都在 -0.6 到 0.6 之间，执行相应的逻辑
                ROS_INFO("the position is ok");
                // 在程序结束前切换到保持模式
                /*
                AUTO.LOITER     保持当前位置模式
                AUTO.RTL        自动返航模式
                AUTO.TAKEOFF    自动起飞模式
                AUTO.LAND       自动降落模式
                AUTO.MISSION    执行预设航点任务模式
                AUTO.RTGS       自动返回到航点（Return to GPS）
                AUTO.READY      准备模式。此模式表示无人机已准备好执行任务，但尚未启动。
                */
                mavros_msgs::SetMode hold_mode;
                hold_mode.request.custom_mode = "AUTO.LOITER";  //AUTO.LOITER:保持当前位置模式
                if (set_mode_client.call(hold_mode) && hold_mode.response.mode_sent) {
                    ROS_INFO("Switched to LOITER mode. The drone will maintain its position.");
                }
                break;
            }
            last_local_pos_time = ros::Time::now();//更新记录无人机姿态（如位置）的时间
        }
        ros::spinOnce();//当spinOnce函数被调用时，会调用回调函数队列中第一个回调函数，这里回调函数是state_cb函数
        rate.sleep();//根据前面ros::Rate rate(20.0);制定的发送频率自动休眠 休眠时间 = 1/频率
    }
    return 0;
}





